/* University of Saskathewan Space Design Team Quadcopter
This is the program developed to be loaded onto an Arduino based Quadcotper built and designed by the USST for the USST.

*/

//Libraries to include
#include <Servo.h>
#include <Wire.h>
#include "Kalman.h"
#include "PID.h"

//definement of global variables. This allows for these variables to be used wherever and reduce the need to pass variables from function to function


float controller[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};//variables read in from the commands sent from the computer
float baseSpeed = 0; //base speed. currently for testing purposes

// Defining the variables for the motors to be servos according to the included servo library
Servo esc1; 
Servo esc2;
Servo esc3;
Servo esc4;

// The variable to be fed to the esc's to allow for the arming sequence to follow through
float armingSpeed = 30;

//defining and initializing the speeds of the motors
float speed1 = armingSpeed;
float speed2 = armingSpeed;
float speed3 = armingSpeed; 
float speed4 = armingSpeed;

//variables to increase the base speed of the quadcopter
float stabilizerScaler =  0.9;//a value to be fed into the PID to scale the output values to reasonable motor values
float rollGain = 0;
float pitchGain = 0;
float yawGain = 0;
float throttleAcceleration = 0;

//for start up testing purposes
int lastTime = 0;
int currentTime = 0;
int difference = 0;

//constants for PID controller
float Kp = 0.18; //for proportinal 
float Ki = 0.0; //for integrator 
float Kd = 0; //for derivative 

//PID rollPID();//declare a PID function for the PID controller

PID rollPID;
PID pitchPID;

/*
Variables for the Kalman filter/IMU to operate
Define kalman functions from the Kalman library
*/



Kalman kalmanX;
Kalman kalmanY;

uint8_t IMUAddress = 0x68;

/* IMU Data*/
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;

double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;

uint32_t timer;




void setup(){
  //Begin Serial communication
  Serial.begin(9600);
  //Attach motors
  esc1.attach(10);
  esc2.attach(11);
  esc3.attach(8);
  esc4.attach(9);  
  
  
  
  //Arm Motors
  while (millis() < 6000){
    speed1 = speed2 = speed3 = speed4 = armingSpeed;
    esc1.write(speed1);
    esc2.write(speed2);
    esc3.write(speed3);
    esc4.write(speed4);
    }
    
    Serial.println("Motor's Armed");
  //Set up IMU  
  
  Wire.begin();
    Serial.println("IMu Step 1");
  i2cWrite(0x6B,0x00); // Disable sleep mode
      Serial.println("IMu Step 2");
      Serial.println(i2cRead(0x75,1)[0]);
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register    
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }
  
    Serial.println("IMu Step 3");
  kalmanX.setAngle(180); // Set starting angle
  kalmanY.setAngle(180);
  timer = micros(); 
 
 Serial.println("IMU Loaded"); 
 
  
}

void loop(){
   
   if (Serial.find("A") == true){
    
      for (int k = 0; k < 10; k ++){  
         controller[k] = Serial.parseFloat();    
      } 
    }
    
    
    //call all the functions to determine the values needed to fly the helicopter
    baseSpeedControl(controller[6], controller[7]);
    modifyConstants(controller[2],controller[4],controller[8],controller[9]);
    IMU();
    rollControl(180);
    //pitchControl(180);
    //yawControl();
    throttleControl(controller[0], controller[1]);
    speedWrite();
    
}

/*************************************************
Function that will determine the base speed of the quadcopter, this can also be described as the speed to maintain a hover
*************************************************/

void baseSpeedControl(int stepDecrease, int stepIncrease){ 
 
  if (stepDecrease == 1){
    baseSpeed = baseSpeed - 2;
  }
  else if (stepIncrease == 1){
    baseSpeed = baseSpeed + 2;
  }
  
  baseSpeed = constrain (baseSpeed,0,100);
  
}

/*************************************************
Function to control the roll axis/stabilize it to the taget pitch angle
*************************************************/
void rollControl(float targetAngle){ //the roll target value comes from the left thumstick, x direction.

  //The actual value is taken from the IMUdata. It is stored in a public variable
  
  rollGain = rollPID.Calculate(kalAngleX,targetAngle,stabilizerScaler,Kd,Ki,Kp); 

  rollGain = constrain(rollGain,-20,20);
  
  Serial.print(rollGain);
  Serial.print(" ");
}

/*************************************************
Function to control the pitch axis/stabilize it to the taget pitch angle
*************************************************/
void pitchControl(float targetAngle){
  
  //The actual value is taken from the IMUdata. It is stored in a public variable
  pitchGain = pitchPID.Calculate(kalAngleY,targetAngle,stabilizerScaler,Kd,Ki,Kp); 

  pitchGain = constrain(pitchGain,-10,10);
  
  Serial.print(pitchGain);
  Serial.print(" ");
  

}

/*************************************************
Function to control the yaw axis/control the rate of spin around the yaw axis
*************************************************/
void yawControl(){
}

/*************************************************
Function to calculate the amount of throttle based on the XBox controller trigger inputs
*************************************************/
void throttleControl(float decrease, float increase){
  int maxThrottle = 20; //this is the maximum throttle allowable by the trigger button

  //increase = increase * 100; //this is multiplied by 100 because it is necessary to maintain precision in the mapping function which only uses integers
  //decrease = decrease * 100;
  
  if (increase == 0 & decrease == 0){
    throttleAcceleration = 0;
  }
  else if(decrease != 0) {
    throttleAcceleration = -1 * map(decrease,0,100,0,maxThrottle);
  }
  else if(increase !=0){
    throttleAcceleration = map(increase,0,100,0,maxThrottle);
  }
  

}

/*************************************************
IMU Value calculator, Make sure the results values
are public variables.
*************************************************/
void IMU(){
/* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);
  tempRaw = ((data[6] << 8) | data[7]);
  gyroX = ((data[8] << 8) | data[9]);
  gyroY = ((data[10] << 8) | data[11]);
  gyroZ = ((data[12] << 8) | data[13]);
  
  //adding in experimental offsets for zeroing our particular IMU
  accX -= 1000;
  accY += 737;
  gyroX += 131;
  gyroY -= 262;
  
  
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
  
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
  
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
  
  temp = ((double)tempRaw + 12412.0) / 340.0;
  
  kalAngleX -=2.2;
  
  
  Serial.print(" R:");
  Serial.print(kalAngleX);
  Serial.print(" ");
  Serial.print(" P:");
  Serial.print(kalAngleY);
  Serial.print(" ");
  //Serial.print("\n");
  
  delay(1);
}

void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
} 


/*************************************************
Function to send the commands to the esc's to controll the motors.
Sum the speed of each motor based on all the above functions
*************************************************/

void modifyConstants(float KdGain, float KiGain, float KpDecrease,float KpIncrease)
{
  KpDecrease *=0.01;
  KpIncrease *=0.01;
  KdGain *=0.1;
//Ki += KiGain;
Kd += KdGain;
Kp += KpIncrease;
Kp -= KpDecrease;

Serial.print(Kp);
Serial.print("  ");
Serial.print(Ki);
Serial.print("  ");
Serial.print(Kd);
Serial.print("  ");

}
void speedWrite(){
  //definement of variables, these values are dependent on the specific motors/esc's/propellers used.
  
  int Max = 94;
  int Min = 61;
  
  /*summing the total speed
    Whether the Gains are added or subtracted is always for whether or not it produces a positive gain
    in the respective axis or direction
    
    The speed needs to be constrained to a range of 0% throttle and 100% throttle
  */
    speed1 = baseSpeed + throttleAcceleration - rollGain + pitchGain - yawGain;
    speed2 = baseSpeed + throttleAcceleration - rollGain - pitchGain + yawGain - 2;
    speed3 = baseSpeed + throttleAcceleration + rollGain - pitchGain - yawGain;
    speed4 = baseSpeed + throttleAcceleration + rollGain + pitchGain + yawGain;
    
    speed1 = constrain(speed1,0,100);
    speed2 = constrain(speed2,0,100);
    speed3 = constrain(speed3,0,100);
    speed4 = constrain(speed4,0,100);
    
    //output throttle
    Serial.print("  ");
    Serial.print(speed1); 
    Serial.print("  ");
    Serial.print(speed2);
    Serial.print("  ");
    Serial.print(speed3);
    Serial.print("  ");
    Serial.print (speed4); 
    Serial.print("\n");

    /*
    The speed in all the above functions are treated in terms of percent throttle.
     From testing we know there is a maximum and minimum value that the motors operate at.
     The throttle will be mapped to a range of four times these values. After mapping the values to this range, 
     the values are quartered so the are properly sized for the values sent to the speed controllers.
     The mapping to a larger value and then quartering allows for more accuracy as the mapping function 
     only allows for the usage of integers, which results in a loss of precision
    */
    
    Max = 4*Max;
    Min = 4*Min;

    speed1 = map(speed1,0,100,Min,Max);
    speed2 = map(speed2,0,100,Min,Max);
    speed3 = map(speed3,0,100,Min,Max);
    speed4 = map(speed4,0,100,Min,Max);

    speed1 = 0.25*speed1;
    speed2 = 0.25*speed2;    
    speed3 = 0.25*speed3;
    speed4 = 0.25*speed4;

    //writing the speeds to the ESC's to control the motors accordingly 
    esc1.write(speed1);
    esc2.write(speed2);
    esc3.write(speed3);
    esc4.write(speed4);
  
}


